#ifndef NAV2_COSTMAP_2D__VORONOI_LAYER_HPP_
#define NAV2_COSTMAP_2D__VORONOI_LAYER_HPP_

#include <memory>
#include <mutex>  // NOLINT

#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_costmap_2d/layer.hpp"
#include "nav2_costmap_2d/layered_costmap.hpp"

#include "dynamicvoronoi/dynamicvoronoi.h"
#include <nav_msgs/msg/occupancy_grid.hpp>
#include "rclcpp/rclcpp.hpp"
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
#include <nav2_costmap_2d/costmap_2d.hpp>

namespace nav2_costmap_2d 
{
class VoronoiLayer : public nav2_costmap_2d::Layer
{
public:
    VoronoiLayer() = default;
    ~VoronoiLayer() override = default;

    virtual void reset();
    void onInitialize() override;
    void updateBounds(double robot_x, double robot_y, double robot_yaw,
                      double* min_x, double* min_y, double* max_x,
                      double* max_y) override;
    void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
                     int max_i, int max_j) override;
    const DynamicVoronoi& voronoi() const { return voronoi_; }
    virtual bool isClearable() {return false;}
    std::mutex& mutex() { return mutex_; }
private:
    static bool OutlineMap(const nav2_costmap_2d::Costmap2D& master_grid,
                           uint8_t value);
    void UpdateDynamicVoronoi(const nav2_costmap_2d::Costmap2D& master_grid);
    DynamicVoronoi voronoi_;
    unsigned int last_size_x_ = 0;
    unsigned int last_size_y_ = 0;
    std::mutex mutex_;
    int visualize_counter_ = 0;
    bool update_enabled_ = true;
};

}
#endif